/*
 * pid.c
 *
 *  Created on: Dec 31, 2023
 *      Author: 80424
 */


#include "pid.h"
PIDType POSE_PIDStruct={.Kp=0.098,.Ki=0.004,.Kd=0.6,.limit=50,.target=2048};
PIDType SPEED_PIDSreuct={.Kp=0.55,.Ki=0.003,.Kd=3,.limit=44};
/*
 * 位置式PID
 * */
void pid_pos_update(PIDType *PID)
{
	float pe, ie, de;//误差比例 误差积分 误差微分
	float out=0;

	/*数据准备*/

	PID->error_now = PID->target - PID->feedback;	//计算当前误差

	if(	PID->error_sum>=8192 && PID->error_now<0)//积分上限 只允许减
	{PID->error_sum += PID->error_now;}	//误差积分

	if(	PID->error_sum<=-8192 && PID->error_now>0)//积分下限 只允许增
	{PID->error_sum += PID->error_now;}

	if(	PID->error_sum <= 8192 &&	PID->error_sum >= -8192)//不大不小 可增可减
	{PID->error_sum += PID->error_now;}
	//如果都不是的 就不处理
	/*输入传入*/
	de = PID->error_now - PID->error_last;	//输入本次的误差微分
	pe = PID->error_now;					//输入本次的误差比例值
	ie = PID->error_sum;					//输入本次的误差积分值

	/*历史值更新*/
	PID->error_last = PID->error_now;//更新误差值

	out = pe*(PID->Kp) + ie*(PID->Ki) + de*(PID->Kd);//PID计算输出

	out = range(out, (-PID->limit), PID->limit);//输出限幅 保证其输出在限制之内
	PID->out = out;
}

float fc = 2.0f;     //截止频率
float Ts = 0.000025;    //采样周期
float pi = 3.14159f; //π
float alpha = 0;     //滤波系数

/************************ 滤波器初始化 alpha *****************************/
void low_pass_filter_init(void)
{
  float b = 2.0 * pi * fc * EEtime*0.01;
  alpha = b / (b + 1);
}

float low_pass_filter(float value)
{
  static float out_last = 0; //上一次滤波值
  float out;

  /***************** 如果第一次进入，则给 out_last 赋值 ******************/
  static char fisrt_flag = 1;
  if (fisrt_flag == 1)
  {
    fisrt_flag = 0;
    out_last = value;
  }

  /*************************** 一阶滤波 *********************************/
  out = out_last + alpha * (value - out_last);
  out_last = out;

  return out;
}
